﻿
#include "BoundingFrustum.h"

#include <Riccsson/Xna/Framework/BoundingBox.h>
#include <Riccsson/Xna/Framework/BoundingSphere.h>

using namespace Riccsson::System;
using namespace Riccsson::Xna::Framework;

BoundingFrustum::BoundingFrustum(Matrix value)
    : PROP3_INIT(BoundingFrustum, matrix)
	, PROP3GET_INIT(BoundingFrustum, Near)
	, PROP3GET_INIT(BoundingFrustum, Far)
	, PROP3GET_INIT(BoundingFrustum, Left)
	, PROP3GET_INIT(BoundingFrustum, Right)
	, PROP3GET_INIT(BoundingFrustum, Top)
	, PROP3GET_INIT(BoundingFrustum, Bottom)
{
	this->_matrix = value;
	this->CreatePlanes();
	this->CreateCorners();
}
PROP3_GET_CPP(BoundingFrustum, Matrix, matrix)
{
	return this->_matrix;
}
PROP3_SET_CPP(BoundingFrustum, Matrix, matrix)
{
	this->_matrix = value;
	this->CreatePlanes();    // FIXME: The odds are the planes will be used a lot more often than the matrix
    this->CreateCorners();   // is updated, so this should help performance. I hope ;)
}
PROP3_GET_CPP(BoundingFrustum, Plane, Near)
{
	return (**this->planes)[0];
}
PROP3_GET_CPP(BoundingFrustum, Plane, Far)
{
	return (**this->planes)[1];
}
PROP3_GET_CPP(BoundingFrustum, Plane, Left)
{
	return (**this->planes)[2];
}
PROP3_GET_CPP(BoundingFrustum, Plane, Right)
{
	return (**this->planes)[3];
}
PROP3_GET_CPP(BoundingFrustum, Plane, Top)
{
	return (**this->planes)[4];
}
PROP3_GET_CPP(BoundingFrustum, Plane, Bottom)
{
	return (**this->planes)[5];
}
ContainmentType BoundingFrustum::Contains(BoundingBox& box)
{
	//var result = default(ContainmentType);
	//this->Contains(ref box, out result);
	//return result;
    throw;
}
void BoundingFrustum::Contains(BoundingBox ref box, ContainmentType out result)
{
	//var intersects = false;
	//for (var i = 0; i < PlaneCount; ++i)
	//{
	//	var planeIntersectionType = default(PlaneIntersectionType);
	//	box.Intersects(/*ref*/ this->planes[i], /*out*/ planeIntersectionType);
	//	switch (planeIntersectionType)
	//	{
	//	case PlaneIntersectionType::Front:
	//		result = ContainmentType::Disjoint; 
	//		return;
	//	case PlaneIntersectionType::Intersecting:
	//		intersects = true;
	//		break;
	//	}
	//}
	//result = intersects ? ContainmentType::Intersects : ContainmentType::Contains;
    throw;
}
ContainmentType BoundingFrustum::Contains(BoundingFrustum frustum)
{
    // TODO: Fixa byggfelen. Alla... :)
	if (*this == frustum)                // We check to see if the two frustums are equal
		return ContainmentType::Contains;// If they are, there's no need to go any further.

	throw Riccsson::System::NotImplementedException();
}
ContainmentType BoundingFrustum::Contains(BoundingSphere& sphere)
{
	ContainmentType result = ContainmentType::Disjoint;
	this->Contains(/*ref*/ sphere, /*out*/ result);
	return result;
}
void BoundingFrustum::Contains(BoundingSphere ref sphere, ContainmentType out result)
{
	bool intersects = false;
	for (int i = 0; i < PlaneCount; ++i) 
	{
		PlaneIntersectionType planeIntersectionType = PlaneIntersectionType::Front;

		// TODO: we might want to inline this for performance reasons
		sphere.Intersects(/*ref*/ (**this->planes)[i], /*out*/ planeIntersectionType);
		switch (planeIntersectionType)
		{
		case PlaneIntersectionType::Front:
			result = ContainmentType::Disjoint; 
			return;
		case PlaneIntersectionType::Intersecting:
			intersects = true;
			break;
		}
	}
	result = intersects ? ContainmentType::Intersects : ContainmentType::Contains;
}
ContainmentType BoundingFrustum::Contains(Vector3 point)
{
	//var result = default(ContainmentType);
	//this->Contains(ref point, out result);
	//return result;
    throw;
}
void BoundingFrustum::Contains(Vector3 ref point, ContainmentType out result)
{
	for (int i = 0; i < PlaneCount; ++i)
	{
		// TODO: we might want to inline this for performance reasons
		if (PlaneHelper::ClassifyPoint(/*ref*/ point, /*ref*/ (**this->planes)[i]) > 0)
		{
			result = ContainmentType::Disjoint;
			return;
		}
	}
	result = ContainmentType::Contains;
}
bool BoundingFrustum::Equals(BoundingFrustum* other)
{
	return (this == other);
}
bool BoundingFrustum::Equals(Riccsson::System::object* obj)
{
	BoundingFrustum* f = (BoundingFrustum*)obj;
	return (Riccsson::System::object::Equals(f, null)) ? false : (this == f);
}
GC_PTR<Riccsson::System::Array<Vector3>> BoundingFrustum::GetCorners()
{
	return (Riccsson::System::Array<Vector3>*)*this->corners->Clone();
}
void BoundingFrustum::GetCorners(Riccsson::System::Array<Vector3>* corners)
{
	if (corners == null) throw Riccsson::System::ArgumentNullException("corners");
	if (corners->Length < CornerCount) throw Riccsson::System::ArgumentOutOfRangeException("corners");

	this->corners->CopyTo(corners, 0);
}
int BoundingFrustum::GetHashCode()
{
	return this->matrix.get().GetHashCode();
}
bool BoundingFrustum::Intersects(BoundingBox& box)
{
	bool result = false;
	this->Intersects(/*ref*/ box, /*out*/ result);
	return result;
}
void BoundingFrustum::Intersects(BoundingBox ref box, bool out result)
{
	ContainmentType containment = ContainmentType::Disjoint; // Defalut.
	this->Contains(/*ref*/ box, /*out*/ containment);
	result = containment != ContainmentType::Disjoint;
}
bool BoundingFrustum::Intersects(BoundingFrustum frustum)
{
	throw Riccsson::System::NotImplementedException();
}
bool BoundingFrustum::Intersects(BoundingSphere& sphere)
{
	bool result = false;
	this->Intersects(/*ref*/ sphere, /*out*/ result);
	return result;
}
void BoundingFrustum::Intersects(BoundingSphere ref sphere, bool out result)
{
	ContainmentType containment = ContainmentType::Disjoint;
	this->Contains(/*ref*/ sphere, /*out*/ containment);
	result = containment != ContainmentType::Disjoint;
}
PlaneIntersectionType BoundingFrustum::Intersects(Plane plane)
{
	throw Riccsson::System::NotImplementedException();
}
void BoundingFrustum::Intersects(Plane ref plane, PlaneIntersectionType out result)
{
	throw Riccsson::System::NotImplementedException();
}
Riccsson::System::Nullable<float> BoundingFrustum::Intersects(Ray& ray)
{
	throw Riccsson::System::NotImplementedException();
}
void BoundingFrustum::Intersects(Ray ref ray, Riccsson::System::Nullable<float> out result)
{
	throw Riccsson::System::NotImplementedException();
}
GC_PTR<Riccsson::System::string> BoundingFrustum::ToString()
{
	Riccsson::System::Text::StringBuilder sb(256);
	sb.Append("{Near:");
	sb.Append((**this->planes)[0].ToString());
	sb.Append(" Far:");
	sb.Append((**this->planes)[1].ToString());
	sb.Append(" Left:");
	sb.Append((**this->planes)[2].ToString());
	sb.Append(" Right:");
	sb.Append((**this->planes)[3].ToString());
	sb.Append(" Top:");
	sb.Append((**this->planes)[4].ToString());
	sb.Append(" Bottom:");
	sb.Append((**this->planes)[5].ToString());
	sb.Append("}");
	return sb.ToString();
}
void BoundingFrustum::CreateCorners()
{
	this->corners = new Riccsson::System::Array<Vector3>(CornerCount);
	IntersectionPoint(/*ref*/ (**this->planes)[0], /*ref*/ (**this->planes)[2], /*ref*/ (**this->planes)[4], /*out*/ (**this->corners)[0]);
	IntersectionPoint(/*ref*/ (**this->planes)[0], /*ref*/ (**this->planes)[3], /*ref*/ (**this->planes)[4], /*out*/ (**this->corners)[1]);
	IntersectionPoint(/*ref*/ (**this->planes)[0], /*ref*/ (**this->planes)[3], /*ref*/ (**this->planes)[5], /*out*/ (**this->corners)[2]);
	IntersectionPoint(/*ref*/ (**this->planes)[0], /*ref*/ (**this->planes)[2], /*ref*/ (**this->planes)[5], /*out*/ (**this->corners)[3]);
	IntersectionPoint(/*ref*/ (**this->planes)[1], /*ref*/ (**this->planes)[2], /*ref*/ (**this->planes)[4], /*out*/ (**this->corners)[4]);
	IntersectionPoint(/*ref*/ (**this->planes)[1], /*ref*/ (**this->planes)[3], /*ref*/ (**this->planes)[4], /*out*/ (**this->corners)[5]);
	IntersectionPoint(/*ref*/ (**this->planes)[1], /*ref*/ (**this->planes)[3], /*ref*/ (**this->planes)[5], /*out*/ (**this->corners)[6]);
	IntersectionPoint(/*ref*/ (**this->planes)[1], /*ref*/ (**this->planes)[2], /*ref*/ (**this->planes)[5], /*out*/ (**this->corners)[7]);
}
void BoundingFrustum::CreatePlanes()
{
	this->planes = new Riccsson::System::Array<Plane>(PlaneCount);
	(**this->planes)[0] = Plane(-this->matrix.get().M13, -this->matrix.get().M23, -this->matrix.get().M33, -this->matrix.get().M43);
	(**this->planes)[1] = Plane(this->matrix.get().M13 - this->matrix.get().M14, this->matrix.get().M23 - this->matrix.get().M24, this->matrix.get().M33 - this->matrix.get().M34, this->matrix.get().M43 - this->matrix.get().M44);
	(**this->planes)[2] = Plane(-this->matrix.get().M14 - this->matrix.get().M11, -this->matrix.get().M24 - this->matrix.get().M21, -this->matrix.get().M34 - this->matrix.get().M31, -this->matrix.get().M44 - this->matrix.get().M41);
	(**this->planes)[3] = Plane(this->matrix.get().M11 - this->matrix.get().M14, this->matrix.get().M21 - this->matrix.get().M24, this->matrix.get().M31 - this->matrix.get().M34, this->matrix.get().M41 - this->matrix.get().M44);
	(**this->planes)[4] = Plane(this->matrix.get().M12 - this->matrix.get().M14, this->matrix.get().M22 - this->matrix.get().M24, this->matrix.get().M32 - this->matrix.get().M34, this->matrix.get().M42 - this->matrix.get().M44);
	(**this->planes)[5] = Plane(-this->matrix.get().M14 - this->matrix.get().M12, -this->matrix.get().M24 - this->matrix.get().M22, -this->matrix.get().M34 - this->matrix.get().M32, -this->matrix.get().M44 - this->matrix.get().M42);
            
	this->NormalizePlane(/*ref*/ (**this->planes)[0]);
	this->NormalizePlane(/*ref*/ (**this->planes)[1]);
	this->NormalizePlane(/*ref*/ (**this->planes)[2]);
	this->NormalizePlane(/*ref*/ (**this->planes)[3]);
	this->NormalizePlane(/*ref*/ (**this->planes)[4]);
	this->NormalizePlane(/*ref*/ (**this->planes)[5]);
}
void BoundingFrustum::IntersectionPoint(Plane ref a, Plane ref b, Plane ref c, Vector3 out result)
{
	// Formula used
	//                d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
	//P =   -------------------------------------------------------------------------
	//                             N1 . ( N2 * N3 )
	//
	// Note: N refers to the normal, d refers to the displacement. '.' means dot product. '*' means cross product
            
	Vector3 v1, v2, v3;
	Vector3 cross;
            
	Vector3::Cross(/*ref*/ b.Normal, /*ref*/ c.Normal, /*out*/ cross);
            
	float f;
	Vector3::Dot(/*ref*/ a.Normal, /*ref*/ cross, /*out*/ f);
	f *= -1.0f;
            
	Vector3::Cross(/*ref*/ b.Normal, /*ref*/ c.Normal, /*out*/ cross);
	Vector3::Multiply(/*ref*/ cross, a.D, /*out*/ v1);
	//v1 = (a.D * (Vector3::Cross(b.Normal, c.Normal)));
            
            
	Vector3::Cross(/*ref*/ c.Normal, /*ref*/ a.Normal, /*out*/ cross);
	Vector3::Multiply(/*ref*/ cross, b.D, /*out*/ v2);
	//v2 = (b.D * (Vector3::Cross(c.Normal, a.Normal)));
            
            
	Vector3::Cross(/*ref*/ a.Normal, /*ref*/ b.Normal, /*out*/ cross);
	Vector3::Multiply(/*ref*/ cross, c.D, /*out*/ v3);
	//v3 = (c.D * (Vector3::Cross(a.Normal, b.Normal)));
            
	result.X = (v1.X + v2.X + v3.X) / f;
	result.Y = (v1.Y + v2.Y + v3.Y) / f;
	result.Z = (v1.Z + v2.Z + v3.Z) / f;
}
void BoundingFrustum::NormalizePlane(Plane ref p)
{
	float factor = 1.0f / p.Normal.Length();
	p.Normal.X *= factor;
	p.Normal.Y *= factor;
	p.Normal.Z *= factor;
	p.D *= factor;
}